#include <mex.h>
#include<matrix.h>
#include  "colorAndResponse.hpp"
#include <math.h>


void cpVectorDouble2Float(float * p_dest, double *p_src, int num_elem){
	for( int i = 0;i<num_elem; ++i)
		p_dest[i]=(float)p_src[i];
}


void *getPtrToField(const mxArray *p_struct, int i, const char* field_name){
	const mxArray *p_field=mxGetField(p_struct,i,field_name);
	return (void*)mxGetPr(p_field);
}

void mexFunction( int nlhs, mxArray *plhs[],
				 int nrhs, const mxArray *prhs[] ){
					 // access the variables

					 int i=0;
					 int n_image = floor(mxGetScalar(prhs[i++])+0.5);// n_image 2
					 int n_orient = floor(mxGetScalar(prhs[i++])+0.5);// n_orient 3
					 int n_length = floor(mxGetScalar(prhs[i++])+0.5);// n_length 5

					 int h =(int) floor(mxGetScalar(prhs[i++])+0.5f);
					 // image sizes and zoom ratios
					 double *p_sx = mxGetPr(prhs[i++]);
					 double *p_sy = mxGetPr(prhs[i++]);

					 double *p_zx = mxGetPr(prhs[i++]);
					 double *p_zy = mxGetPr(prhs[i++]);

					 // get color maps
					 float **p_color_l_maps = (float**)mxCalloc((int)(n_image),sizeof(float*));
					 for ( int i_page = 0; i_page<n_image; ++i_page){
						 mxArray *p_page = mxGetCell(prhs[i],i_page);
						 p_color_l_maps[i_page]=(float*)mxGetPr(p_page);
					 }
					 i++;


					 float **p_color_a_maps = (float**)mxCalloc((int)(n_image),sizeof(float*));
					 for ( int i_page = 0; i_page<n_image; ++i_page){
						 mxArray *p_page = mxGetCell(prhs[i],i_page);
						 p_color_a_maps[i_page]=(float*)mxGetPr(p_page);
					 }
					 i++;

					 float **p_color_b_maps = (float**)mxCalloc((int)(n_image),sizeof(float*));
					 for ( int i_page = 0; i_page<n_image; ++i_page){
						 mxArray *p_page = mxGetCell(prhs[i],i_page);
						 p_color_b_maps[i_page]=(float*)mxGetPr(p_page);
					 }
					 i++;



					 float *p_PMat = (float*) mxGetPr(prhs[i++]);
					 float *p_viewMat = (float*)mxGetPr(prhs[i++]);


					 // get ptr to AoT structure
					 const mxArray *p_tan = prhs[i++];
					 if( !mxIsStruct(p_tan)){
						 mexPrintf("Error, not a structure, p_tan.\n");
					 }

					 // get ptr to the terminal node
					 const mxArray *p_term_node=prhs[i++];


					 int *p_arg_max_map =(int*) mxGetPr(prhs[i++]);

					 float gain_th = mxGetScalar(prhs[i++]);  // threshold on turned_on object images;
                                         int n_char = mxGetN(prhs[i]);
                                         char *file_name =(char*)malloc(sizeof(char)*n_char);
                                         mxGetString(prhs[i++],file_name,n_char+1);
                                         const char* p_file_name =const_cast<const char*>(file_name);



					 /*******************************
					 *finished grabbing pointers from prhs
					 ********************************/
					 double *p_root_tan_rotation = (double*)getPtrToField(p_tan, 0, "rotationMat");
					 float root_tan_rotation[9];
					 cpVectorDouble2Float(root_tan_rotation,p_root_tan_rotation,9);

					 double *p_root_tan_origin =(double*) getPtrToField(p_tan, 0, "origin");

					 float root_tan_T[3];
					 // note that according to notes, T is the negative of the tan origin.
					 for(int j =0;j<3; ++j) root_tan_T[j] = - p_root_tan_origin[j];

					 double root_tan_scale = *(double*)getPtrToField(p_tan, 0, "swt"); //


					 const mxArray *p_plane = mxGetField(p_term_node,0,"plane");
					 // get plane parameters, and convert to internal representation
					 plane target_plane;

					 double *p_tmp_array=(double*)getPtrToField(p_plane,0,"nt");
					 cpVectorDouble2Float(&target_plane.nt[0],p_tmp_array,4);
					 p_tmp_array = (double*)getPtrToField(p_plane, 0, "origin");
					 cpVectorDouble2Float(&target_plane.origin[0],p_tmp_array,3);
					 p_tmp_array = (double*)getPtrToField(p_plane, 0, "u");
					 cpVectorDouble2Float(&target_plane.bu[0],p_tmp_array,3);
					 p_tmp_array = (double*)getPtrToField(p_plane,0,"v");
					 cpVectorDouble2Float(&target_plane.bv[0],p_tmp_array,3);
					 target_plane.width = *(double*)getPtrToField(p_plane,0,"w");
					 target_plane.height = *(double*)getPtrToField(p_plane,0,"h");
					 target_plane.stp =*(double*)getPtrToField(p_plane,0,"stp");


					 target_plane.height*=target_plane.stp; // plane size in plane coordinate system
					 target_plane.width *= target_plane.stp;



					 // begin the main computation part
					 colorAndResponse color_model;
					 color_model.initializeGrids(target_plane.width,target_plane.height,h,n_orient,n_image);
					 color_model.camera.initialize(root_tan_scale,root_tan_rotation,root_tan_T,n_orient,n_length,h);
					 color_model.camera.updatePlane(target_plane.bu,target_plane.bv,target_plane.origin,target_plane.nt,target_plane.stp,target_plane.width,target_plane.height);
					 color_model.computeColorResponse(p_color_a_maps,p_color_b_maps,p_color_l_maps,p_PMat,p_viewMat,p_sx,p_sy,p_zx,p_zy,p_arg_max_map);

					 color_model.fitGaussianModel(gain_th);
					 color_model.fitHistogram();
					 color_model.outputInCanonicalView(p_file_name);

					 // free memory
					 color_model.release();
					 mxFree((void*)p_color_l_maps);
					 mxFree((void*)p_color_a_maps);
					 mxFree((void*)p_color_b_maps);
					free(file_name);
}
